/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <boost/foreach.hpp>

#include <matchmaker/server.h>

using namespace std;

namespace matchmaker {


Server::Server()
{

}


void Server::run()
{
  nh_ = boost::make_shared<ros::NodeHandle>();

  waitForClock();

  advertSub_ = nh_->subscribe<matchmaker_msgs::Advert>("soa/advert", 100,
      boost::bind(&Server::handleAdvertMsg, this, _1));

  findServicesServer_ = nh_->advertiseService("soa/find", &Server::handleFindServices, this);

  getServiceStatusServer_ = nh_->advertiseService("soa/get_status", &Server::handleGetServiceStatus, this);

  expiryCheckTimer_ = nh_->createTimer(ros::Duration(EXPIRY_CHECK_PERIOD),
      &Server::checkExpiredCB, this);

  serverUpdateSendTimer_ = nh_->createTimer(ros::Duration(SERVER_UDPATE_MSG_SEND_PERIOD),
      boost::bind(&Server::sendAllServerUpdateMsgs, this));

  clientUpdateMsgSub_ = nh_->subscribe<matchmaker_msgs::ClientUpdate>(
      "soa/client_update", 100, boost::bind(&Server::handleClientUpdateMsg, this, _1));

  ROS_INFO_STREAM("Matchmaker server running, version " << getVersionString());

  while (ros::ok()) {
    ros::spinOnce();
    ros::WallDuration(0.01).sleep();
  }

  std::cout << std::endl << "Matchmaker exiting." << std::endl;
}


std::string Server::getVersionString()
{
  static const std::string version(MATCHMAKER_VERSION);
  return version;
}


void Server::waitForClock()
{
  bool use_sim_time;
  ros::param::param("/use_sim_time", use_sim_time, false);
  // If we're using simulation time, wait for the clock to be published before going any further.
  if (use_sim_time) {
    const ros::WallDuration diagPeriod(1);
    // Set diagTime in the future so diag won't be seen in most cases.
    ros::WallTime diagTime = ros::WallTime::now() + diagPeriod;
    while (ros::ok() && ros::Time::now().sec == 0) {
      if (ros::WallTime::now() > diagTime) {
        // Can't use throttled version of ROS_INFO because it will use sim time.
        ROS_INFO_STREAM("Waiting for first ROS simulation clock message.");
        diagTime += diagPeriod;
      }
      ros::WallDuration(0.01).sleep();
    }

    if (!ros::ok()) {
      ROS_WARN_STREAM("!ros::ok() whilst waiting for simulation clock.");
      return;
    }
  }
  else {
    if (ros::Time::now().sec == 0) {
      ROS_ERROR_STREAM("Param use_sim_time is not true, but time is zero!");
      exit(-1);
    }
  }
}


void Server::checkExpiredCB(const ros::TimerEvent& event)
{
  checkExpiredAdverts();
  checkExpiredClients();
}


void Server::checkExpiredAdverts()
{
  ROS_DEBUG_STREAM("Checking for expired adverts.");

  std::vector<ServiceID> expired;
  ros::Time timeNow = ros::Time::now();

  BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& kvPair, advertMsgMap_) {
    double ageSec = (timeNow - kvPair.second->stamp).toSec();

    if (ageSec > ADVERT_EXPIRY_TIME) {
      std::string id = kvPair.first.toString();
      ServiceStatus status((ServiceStatus::Enum)kvPair.second->status, kvPair.second->extra_status);
      
      if (status.hasValue(matchmaker_msgs::Advert::STATUS_REMOVED)) {
        ROS_INFO_STREAM("Expiring old REMOVED advert (age " << ageSec << " sec) : " << id);
      }
      else {
        ROS_WARN_STREAM("Expiring inactive advert (age " << ageSec << " sec) in state "
            << status.toString() << ": " << id);
      }
      // Add to list to be safely erased below.
      expired.push_back(kvPair.first);
    }
  }

  BOOST_FOREACH(ServiceID& service, expired) {
    advertMsgMap_.erase(service);
    // Send status after erase, as the advert will not be found and REMOVED status will be sent.
    sendServerUpdateMsgsForServiceId(service.toString());
  }

  ROS_DEBUG_STREAM("Service map size: " << advertMsgMap_.size());
}


void Server::checkExpiredClients()
{
  ROS_DEBUG_STREAM("Checking for expired clients.");

  std::vector<std::string> expired;
  ros::Time timeNow = ros::Time::now();

  BOOST_FOREACH(M_StringToClientUpdateMsg::value_type& kvPair, clientUpdateMsgMap_) {
    double ageSec = (timeNow - kvPair.second->stamp).toSec();
    const std::string& clientName = kvPair.first;

    if (ageSec > MONITOR_EXPIRY_TIME) {
      ROS_WARN_STREAM("Expiring inactive client " << clientName << ", age " << ageSec << " sec");
      // Add to list to be safely erased below.
      expired.push_back(kvPair.first);
    }
  }

  BOOST_FOREACH(std::string& clientName, expired) {
    removeClientDetails(clientName);
  }
}


void Server::handleAdvertMsg(const ros::MessageEvent<matchmaker_msgs::Advert const>& event)
{
  //ROS_INFO_STREAM("Received advert.");
  //ROS_INFO_STREAM("  connection header: " << event.getConnectionHeader().
  //ROS_INFO_STREAM("  publisher name: " << event.getPublisherName());

  matchmaker_msgs::AdvertConstPtr msg = event.getConstMessage();

  ServiceID key(event.getPublisherName(), msg->topic);

  if (msg->status_stamp.isZero()) {
    ROS_WARN_STREAM_THROTTLE(2.0, "Ignoring advert with unset status for " << key.toString());
    return;
  }

  ros::Duration msgUpdateAge = event.getReceiptTime() - msg->stamp;
  if (msgUpdateAge.toSec() > ADVERT_EXPIRY_TIME) {
    ROS_WARN_STREAM_THROTTLE(2.0, "Ignoring advert with old stamp field for " << key.toString());
    return;
  }

  M_ServiceToAdvertMsg::iterator previousAdvert = advertMsgMap_.find(key);
  bool sendUpdates = false;
  std::string id = key.toString();
  if (previousAdvert != advertMsgMap_.end()) {
    ROS_DEBUG_STREAM("Replacing existing advert for " << key.toString());
    if (previousAdvert->second->status != msg->status
        || previousAdvert->second->extra_status != msg->extra_status)
    {
      // Only print an info line if the service status or extra status has changed.
      ServiceStatus status((ServiceStatus::Enum)msg->status, msg->extra_status);
      ROS_INFO_STREAM("Service " << id << " - " << status.toString());
      sendUpdates = true;
    }
  }
  else {
    ServiceStatus status((ServiceStatus::Enum)msg->status, msg->extra_status);
    ROS_INFO_STREAM("New service advert " << id << " - " << status.toString());
    sendUpdates = true;
  }

  // Update the map with the latest advert message.
  advertMsgMap_[key] = msg;

  // Check if we need to send status updates, _after_ updating the map with the latest message.
  if (sendUpdates) {
    sendServerUpdateMsgsForServiceId(id);
    sendServerUpdateMsgsForServiceName(msg->service_name);
  }
}


void Server::handleClientUpdateMsg(const ros::MessageEvent<matchmaker_msgs::ClientUpdate const>& event)
{
  std::string clientName = event.getPublisherName();
  matchmaker_msgs::ClientUpdateConstPtr msg = event.getMessage();

  M_StringToClientUpdateMsg::iterator msgIter = clientUpdateMsgMap_.find(clientName);
  bool newClient = msgIter != clientUpdateMsgMap_.end();

  matchmaker_msgs::ClientUpdateConstPtr lastMsg;

  bool topicChanged = false;
  if (newClient) {
    // Monitor message from a node we are already aware of.
    lastMsg = msgIter->second;
    //ROS_INFO_STREAM("Client sent another monitor message: " << name);
    if (msg->status_topic != lastMsg->status_topic) {
      // This is unexpected with the matchmaker Client as it is.
      ROS_WARN_STREAM("Client '" << clientName << "' changed status topic to: " << msg->status_topic);
      topicChanged = true;
    }
  }
  else {
    ROS_INFO_STREAM("Client sent first update message: " << clientName);
    ROS_DEBUG_STREAM("Client '" << clientName << "' status topic: " << msg->status_topic);
    // Node is new, so topic has changed.
    topicChanged = true;
  }

  handleClientMonitoringUpdate(clientName, msg, lastMsg);

  handleClientQueryingUpdate(clientName, msg, lastMsg);

  // Update msg in the map.
  clientUpdateMsgMap_[clientName] = msg;

  // Update the publisher if necessary.
  if (topicChanged) {
    serverUpdatePubMap_[clientName] = nh_->advertise<matchmaker_msgs::ServerUpdate>(
        msg->status_topic, 1, true /* latch last msg */);
    sendServerUpdateMsg(clientName);
  }
}


void Server::handleClientMonitoringUpdate(const std::string& clientName,
    matchmaker_msgs::ClientUpdateConstPtr msg,
    matchmaker_msgs::ClientUpdateConstPtr lastMsg)
{
  if (lastMsg) {
    std::set<std::string> removeSet;  // remove set is Old - New
    std::set<std::string> addSet;     // add set is New - Old
    // Create the remove and add sets.
    BOOST_FOREACH(const std::string & id, lastMsg->ids) { removeSet.insert(id); }
    BOOST_FOREACH(const std::string & id, msg->ids) { removeSet.erase(id); addSet.insert(id); }
    BOOST_FOREACH(const std::string & id, lastMsg->ids) { addSet.erase(id); }

    // Now perform the removals and additions.
    BOOST_FOREACH(const std::string & id, removeSet) {
       removeMonitoredService(clientName, id);
       ROS_INFO_STREAM("Removing monitor by " << clientName << " of service " << id);
    }
    BOOST_FOREACH(const std::string & id, addSet) {
      ROS_INFO_STREAM("Adding monitor by " << clientName << " of service " << id);
      addMonitoredService(clientName, id);
    }
  }
  else {
    // This is new node, so simply add all the monitored services.
    BOOST_FOREACH(std::string id, msg->ids) {
      addMonitoredService(clientName, id);
      ROS_INFO_STREAM("Adding monitor by " << clientName << " of service " << id);
    }
  }
}


void Server::handleClientQueryingUpdate(const std::string& clientName,
    matchmaker_msgs::ClientUpdateConstPtr msg,
    matchmaker_msgs::ClientUpdateConstPtr lastMsg)
{
  if (lastMsg) {
    std::set<std::string> removeSet;  // remove set is Old - New
    std::set<std::string> addSet;     // add set is New - Old
    // Create the remove and add sets.
    BOOST_FOREACH(const std::string & name, lastMsg->names) { removeSet.insert(name); }
    BOOST_FOREACH(const std::string & name, msg->names) { removeSet.erase(name); addSet.insert(name); }
    BOOST_FOREACH(const std::string & name, lastMsg->names) { addSet.erase(name); }

    // Now perform the removals and additions.
    BOOST_FOREACH(const std::string & serviceName, removeSet) {
       removeQueriedService(clientName, serviceName);
       ROS_INFO_STREAM("Removing query by " << clientName << " for service '" << serviceName << "'");
    }
    BOOST_FOREACH(const std::string & serviceName, addSet) {
      ROS_INFO_STREAM("Adding query by " << clientName << " for service '" << serviceName << "'");
      addQueriedService(clientName, serviceName);
    }
  }
  else {
    // This is new node, so simply add all the monitored services.
    BOOST_FOREACH(std::string serviceName, msg->names) {
      addQueriedService(clientName, serviceName);
      ROS_INFO_STREAM("Adding query by " << clientName << " for service '" << serviceName << "'");
    }
  }
}


void Server::removeClientDetails(const std::string & clientName)
{
  M_StringToClientUpdateMsg::iterator msgIter = clientUpdateMsgMap_.find(clientName);
  if (msgIter == clientUpdateMsgMap_.end()) {
    ROS_WARN_STREAM("removeMonitorDetails: Not found in map: " << clientName);
    return;
  }

  const matchmaker_msgs::ClientUpdateConstPtr & msg = msgIter->second;

  ROS_DEBUG_STREAM("Removing monitor details for " << clientName);

  BOOST_FOREACH(std::string id, msg->ids)
  {
    ROS_DEBUG_STREAM("Removing monitor by " << clientName << " of service " << id);
    removeMonitoredService(clientName, id);
  }

  ROS_DEBUG_STREAM("Removing query details for " << clientName);

  BOOST_FOREACH(std::string serviceName, msg->names)
  {
    ROS_DEBUG_STREAM("Removing querying by " << clientName << " for service '" << serviceName << "'");
    removeQueriedService(clientName, serviceName);
  }

  // Remove oneshot timer, if it exists.
  serverUpdateOneshotTimerMap_.erase(clientName);

  // Remove from msg map.
  clientUpdateMsgMap_.erase(msgIter);

  // Remove from pub map.
  serverUpdatePubMap_.erase(clientName);
}


void Server::addMonitoredService(const std::string & clientName, const std::string & serviceId)
{
  M_StringToStringSet::iterator iter = monitoredIdClientMap_.find(serviceId);
  boost::shared_ptr<std::set<std::string> > clientSet;

  if (iter == monitoredIdClientMap_.end()) {
    clientSet = boost::make_shared<std::set<std::string> >();
    monitoredIdClientMap_[serviceId] = clientSet;
  }
  else {
    clientSet = iter->second;
  }

  clientSet->insert(clientName);
}


void Server::removeMonitoredService(const std::string & clientName, const std::string & serviceId)
{
  M_StringToStringSet::iterator iter = monitoredIdClientMap_.find(serviceId);

  if (iter == monitoredIdClientMap_.end()) return;

  iter->second->erase(clientName);

  // Remove the set if it is empty. It is a shared_ptr, so deletion is automatic
  // once all references to it are destroyed.
  if (iter->second->size() == 0) {
    monitoredIdClientMap_.erase(iter);
  }
}


void Server::addQueriedService(const std::string & clientName, const std::string & serviceName)
{
  M_StringToStringSet::iterator iter = queriedNameClientMap_.find(serviceName);
  boost::shared_ptr<std::set<std::string> > clientSet;

  if (iter == queriedNameClientMap_.end()) {
    clientSet = boost::make_shared<std::set<std::string> >();
    queriedNameClientMap_[serviceName] = clientSet;
  }
  else {
    clientSet = iter->second;
  }

  clientSet->insert(clientName);
}


void Server::removeQueriedService(const std::string & clientName, const std::string & serviceName)
{
  M_StringToStringSet::iterator iter = queriedNameClientMap_.find(serviceName);

  if (iter == queriedNameClientMap_.end()) return;

  iter->second->erase(clientName);

  // Remove the set if it is empty. It is a shared_ptr, so deletion is automatic
  // once all references to it are destroyed.
  if (iter->second->size() == 0) {
    queriedNameClientMap_.erase(iter);
  }
}


void Server::sendAllServerUpdateMsgs()
{
  BOOST_FOREACH(const M_StringToPublisher::value_type & pair, serverUpdatePubMap_)
  {
    sendServerUpdateMsg(pair.first);
  }
}


void Server::sendServerUpdateMsgsForServiceId(const std::string & serviceId)
{
  M_StringToStringSet::iterator iter = monitoredIdClientMap_.find(serviceId);
  if (iter != monitoredIdClientMap_.end()) {
    BOOST_FOREACH(const std::string & clientName, *(iter->second))
    {
      // Use near-instantaneous delayed send, to allow of several updates in
      // one message, for efficiency.
      scheduleSendServerUpdateMsg(clientName);
    }
  }
}


void Server::sendServerUpdateMsgsForServiceName(const std::string & serviceName)
{
  // First test for wildcard queries (empty serviceName), that match everything.
  M_StringToStringSet::iterator iter = queriedNameClientMap_.find("");
  if (iter != queriedNameClientMap_.end()) {
    BOOST_FOREACH(const std::string & clientName, *(iter->second))
    {
      // Use near-instantaneous delayed send, to allow of several updates in
      // one message, for efficiency.
      scheduleSendServerUpdateMsg(clientName);
    }
  }

  // Then look for queries matching the service name.
  iter = queriedNameClientMap_.find(serviceName);
  if (iter != queriedNameClientMap_.end()) {
    BOOST_FOREACH(const std::string & clientName, *(iter->second))
    {
      // Use near-instantaneous delayed send, to allow of several updates in
      // one message, for efficiency.
      scheduleSendServerUpdateMsg(clientName);
    }
  }
}


void Server::scheduleSendServerUpdateMsg(const std::string & clientName)
{
  // Schedule a one-shot timer to send the update msg to the client.
  // Setting a timer rather than calling the method directly
  // allows the message to be sent once after receiving several advert
  // updates together, rather than wastefully sending the update after
  // each one.

  M_StringToTimer::iterator iter = serverUpdateOneshotTimerMap_.find(clientName);

  // Only create the timer if one isn't already pending, otherwise ROS gives us a
  // warning for destroying the old one quickly after creation.
  if (iter == serverUpdateOneshotTimerMap_.end() || !iter->second.isValid()) {
    // Timer with tiny duration to expire nearly instantly.
    ros::Timer timer = nh_->createTimer(ros::Duration(SCHEDULED_SEND_TIMEOUT),
        boost::bind(&Server::sendServerUpdateMsg, this, clientName),
        true /* oneshot */);
    serverUpdateOneshotTimerMap_[clientName] = timer;
  }
}


void Server::sendServerUpdateMsg(const std::string & name)
{
  //ROS_INFO_STREAM("sendServiceStatusMsg: Monitoring node: " << name);
  M_StringToPublisher::iterator pubIter = serverUpdatePubMap_.find(name);
  if (pubIter == serverUpdatePubMap_.end()) {
    ROS_ERROR_STREAM("sendServerUpdateMsg: Name not in clientPubMap: " << name);
    return;
  }

  matchmaker_msgs::ServerUpdate serverMsg;

  M_StringToClientUpdateMsg::iterator clientMsgIter = clientUpdateMsgMap_.find(name);

  if (clientMsgIter == clientUpdateMsgMap_.end()) {
    ROS_ERROR_STREAM("sendServerUpdateMsg: Name not in clientUpdateMsgMap: " << name);
    return;
  }

  matchmaker_msgs::ClientUpdateConstPtr clientMsg = clientMsgIter->second;

  fillServerUpdateForMonitors(clientMsg, serverMsg);

  fillServerUpdateForQueries(clientMsg, serverMsg);

  // Publish message to this client node.
  serverMsg.stamp = ros::Time::now();
  pubIter->second.publish(serverMsg);
}


void Server::fillServerUpdateForMonitors(matchmaker_msgs::ClientUpdateConstPtr& clientMsg,
    matchmaker_msgs::ServerUpdate& serverMsg)
{
  int numIds = clientMsg->ids.size();

  // Reserve space in the message vectors, to avoid excess memory allocation.
  serverMsg.ids.reserve(numIds);
  serverMsg.status.reserve(numIds);

  BOOST_FOREACH(const std::string & idString, clientMsg->ids)
  {
    // Convert ID string to ServiceID object.
    ServiceID id(idString);
    if (!id.isValid()) {
      ROS_WARN_STREAM("sendServerUpdateMsg: Ignoring invalid service ID: " << idString);
      continue;
    }

    // Find ID in advert map and add it to the message.
    M_ServiceToAdvertMsg::iterator advertIter = advertMsgMap_.find(id);
    serverMsg.ids.push_back(idString);

    if (advertIter != advertMsgMap_.end()) {
      // Found advert, use status inside.
      serverMsg.status.push_back(advertIter->second->status);
      serverMsg.extra_status.push_back(advertIter->second->extra_status);
    }
    else {
      // No advert, use removed status.
      serverMsg.status.push_back(ServiceStatus::REMOVED);
      serverMsg.extra_status.push_back("");
    }
  }
}


void Server::fillServerUpdateForQueries(matchmaker_msgs::ClientUpdateConstPtr& clientMsg,
    matchmaker_msgs::ServerUpdate& serverMsg)
{
  int numQueries = clientMsg->names.size();

  // Reserve space in the message vectors, to avoid excess memory allocation.
  serverMsg.names.reserve(numQueries);
  serverMsg.num_results.reserve(numQueries);

  BOOST_FOREACH(const std::string & serviceName, clientMsg->names)
  {
    serverMsg.names.push_back(serviceName);

    if (serviceName.length() == 0) {
      BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
        serverMsg.provider_names.push_back(advertPair.first.providerName_);
        serverMsg.adverts.push_back(*(advertPair.second));
      }
      serverMsg.num_results.push_back(advertMsgMap_.size());
    }
    else {
      size_t numResults = 0;
      // TODO: optimise search
      BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
        if (advertPair.second->service_name == serviceName) {
          serverMsg.provider_names.push_back(advertPair.first.providerName_);
          serverMsg.adverts.push_back(*(advertPair.second));
          numResults++;
        }
      }
      serverMsg.num_results.push_back(numResults);
    }
  }
}


bool Server::handleFindServices(matchmaker_msgs::FindServices::Request& request,
    matchmaker_msgs::FindServices::Response& response)
{
  if (request.query_type == matchmaker_msgs::FindServices::Request::QUERY_TYPE_ALL) {
    if (request.services.size() != 0) {
      ROS_WARN_STREAM("Unexpected non-empty services vector in QueryForServices (all) request.");
    }

    BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
      response.provider_names.push_back(advertPair.first.providerName_);
      response.adverts.push_back(*(advertPair.second));
    }

    response.num_results.push_back(response.adverts.size());

    return true;
  }
  else if (request.query_type == matchmaker_msgs::FindServices::Request::QUERY_TYPE_PREFERRED_ALL) {
    if (request.services.size() != 0) {
      ROS_WARN_STREAM("Unexpected non-empty services vector in QueryForServices (pref. all) request.");
    }

    // TODO: Make preferred not just 'first one in the advert map for each service'.
    std::set<std::string> serviceSet;
    M_ServiceToAdvertMsg::iterator advertIter;
    BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
      const std::string &serviceName = advertPair.second->service_name;
      if (serviceSet.find(serviceName) != serviceSet.end()) {
        // Skip services we already have seen an instance of.
        continue;
      }
      serviceSet.insert(serviceName);
      response.provider_names.push_back(advertPair.first.providerName_);
      response.adverts.push_back(*(advertPair.second));
    }

    response.num_results.push_back(response.adverts.size());

    return true;
  }
  else if (request.query_type == matchmaker_msgs::FindServices::Request::QUERY_TYPE_PROVIDING)
  {
    if (request.services.size() == 0) {
      ROS_WARN_STREAM("QueryForServices (all providing) with no service_name!");
      return false;
    }

    BOOST_FOREACH(std::string& name, request.services) {
      size_t numResults = 0;
      // TODO: optimise search
      BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
        if (advertPair.second->service_name == name) {
          response.provider_names.push_back(advertPair.first.providerName_);
          response.adverts.push_back(*(advertPair.second));
          numResults++;
        }
      }
      response.num_results.push_back(numResults);
      //ROS_INFO_STREAM("QueryForServices: " << numResults << " results for <" << name << ">");
    }

    return true;
  }
  else if (request.query_type == matchmaker_msgs::FindServices::Request::QUERY_TYPE_PREFERRED_PROVIDING)
  {
    if (request.services.size() == 0) {
      ROS_WARN_STREAM("QueryForServices (preferred) with no service_name!");
      return false;
    }

    BOOST_FOREACH(std::string& name, request.services) {
      size_t numResults = 0;
      // TODO: optimise search
      BOOST_FOREACH(M_ServiceToAdvertMsg::value_type& advertPair, advertMsgMap_) {
        if (advertPair.second->service_name == name) {
          // TODO: Make preferred mean something other than 'first one we find'.
          response.provider_names.push_back(advertPair.first.providerName_);
          response.adverts.push_back(*(advertPair.second));
          numResults++;
          break;
        }
      }
      response.num_results.push_back(numResults);
      //ROS_INFO_STREAM("QueryForServices: " << numResults << " results for <" << name << ">");
    }

    return true;
  }
  else if (request.query_type == matchmaker_msgs::FindServices::Request::QUERY_TYPE_ID)
  {
    BOOST_FOREACH(std::string& id, request.services){
      ServiceID key(id);

      if (advertMsgMap_.find(id) == advertMsgMap_.end()) {
        ROS_INFO_STREAM("Ignoring unknown service ID in query: " << id);
        continue;
      }

      response.provider_names.push_back(key.providerName_);
      response.adverts.push_back(*advertMsgMap_[key]);
    }
    response.num_results.push_back(response.adverts.size());

    return true;
  }
  else
  {
    ROS_WARN_STREAM("Unsupported QueryForServices query_type: " << request.query_type);
    return false;
  }
}


bool Server::handleGetServiceStatus(matchmaker_msgs::GetServiceStatus::Request& request,
    matchmaker_msgs::GetServiceStatus::Response& response)
{
  // Iterate through the service IDs in the request.
  M_ServiceToAdvertMsg::iterator iter;
  BOOST_FOREACH(std::string stringId, request.ids) {
    ServiceID id(stringId);
    // Get the status from the advert map, or use REMOVED if it isn't there.
    iter = advertMsgMap_.find(id);
    if (iter != advertMsgMap_.end()) {
      response.status.push_back(iter->second->status);
      response.extra_status.push_back(iter->second->extra_status);
    }
    else {
      response.status.push_back(matchmaker_msgs::Advert::STATUS_REMOVED);
      response.extra_status.push_back("");
    }
  }

  ROS_DEBUG_STREAM("handleQueryForServiceStatus: returning status for " << request.ids.size() << " IDs.");

  return true;
}


} // namespace
